#include "N100.h"

uint8_t Fd_data[64];
uint8_t Fd_rsimu[64];
uint8_t Fd_rsahrs[64];
uint8_t rs_imutype = 0;
uint8_t rs_ahrstype = 0;
uint8_t ttl_receive = 0;

static uint8_t Count = 0;
static uint8_t last_rsnum = 0;
static uint8_t rsimu_flag = 0;
static uint8_t rsacc_flag = 0;
uint8_t uart3_rx_byte = 0;

float Roll_Deg, Pitch_Deg, Yaw_Deg;
float yaw_ref = 0;
float yaw_mapped, yaw_fixed;
uint8_t fix_imu_flag = 0;

IMUData_Packet_t IMUData_Packet;
AHRSData_Packet_t AHRSData_Packet;

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart->Instance == USART1)
    {
        if(car_data[0] == 0x11)
        {
            car_forward(100);
        }
		if(car_data[0] == 0x31) // 将当前偏转角记录，以此为基准角，可认为是校准
        {
			fix_imu_flag = 1;
            yaw_ref = Yaw_Deg;
			yaw_fixed = yaw_mapped - yaw_ref;
			printf("yaw_now = %.2f\r\n", yaw_ref);
			
			/* AI写的校准零点后的角度获取 */
//			float raw_relative_yaw = Yaw_Deg - yaw_ref;
//			if (raw_relative_yaw < 0)
//				raw_relative_yaw += 360.0f;
//			else if (raw_relative_yaw >= 360.0f)
//				raw_relative_yaw -= 360.0f;

//			// 再做映射成 [-180, 180]
//			if (raw_relative_yaw >= 180.0f)
//				yaw_fixed = raw_relative_yaw - 360.0f;
//			else
//				yaw_fixed = raw_relative_yaw;

        }
        
        HAL_UART_Receive_IT(&huart1, car_data, 1);
    }
	
    if (huart->Instance == USART3)
    {
        ttl_receive = 1;
        Fd_data[Count] = uart3_rx_byte;

        if (((last_rsnum == FRAME_END) && (uart3_rx_byte == FRAME_HEAD)) || Count > 0)
        {
            Count++;

            if ((Fd_data[1] == TYPE_IMU) && (Fd_data[2] == IMU_LEN))
                rsimu_flag = 1;

            if ((Fd_data[1] == TYPE_AHRS) && (Fd_data[2] == AHRS_LEN))
                rsacc_flag = 1;
        }
        else
        {
            Count = 0;
        }

        last_rsnum = uart3_rx_byte;

        if (rsimu_flag == 1 && Count == IMU_RS)
        {
            Count = 0;
            rsimu_flag = 0;
            rs_imutype = 1;

            if (Fd_data[IMU_RS - 1] == FRAME_END)
                memcpy(Fd_rsimu, Fd_data, sizeof(Fd_data));
        }

        if (rsacc_flag == 1 && Count == AHRS_RS)
        {
            Count = 0;
            rsacc_flag = 0;
            rs_ahrstype = 1;

            if (Fd_data[AHRS_RS - 1] == FRAME_END)
                memcpy(Fd_rsahrs, Fd_data, sizeof(Fd_data));

            for (int i = 0; i < sizeof(Fd_data); i++)
                Fd_data[i] = 0;
        }
//		printf("U3 RX: 0x%02X\r\n", uart3_rx_byte);/* 测试用回显 */

        // 继续接收下一个字节
        HAL_UART_Receive_IT(&huart3, &uart3_rx_byte, 1);
    }
}


uint8_t TTL_Hex2Dec(void)
{
    if (rs_ahrstype == 1)
    {
        if (Fd_rsahrs[1] == TYPE_AHRS && Fd_rsahrs[2] == AHRS_LEN)
        {
            AHRSData_Packet.Roll          = DATA_Trans(Fd_rsahrs[19], Fd_rsahrs[20], Fd_rsahrs[21], Fd_rsahrs[22]);
            AHRSData_Packet.Pitch         = DATA_Trans(Fd_rsahrs[23], Fd_rsahrs[24], Fd_rsahrs[25], Fd_rsahrs[26]);
            AHRSData_Packet.Heading       = DATA_Trans(Fd_rsahrs[27], Fd_rsahrs[28], Fd_rsahrs[29], Fd_rsahrs[30]);

            AHRSData2PC();  // 上传数据
        }
        rs_ahrstype = 0;
    }

    return 0;
}

float DATA_Trans(uint8_t Data_1, uint8_t Data_2, uint8_t Data_3, uint8_t Data_4)
{
    uint32_t raw = 0;
    float result = 0.0f;

    raw |= ((uint32_t)Data_4 << 24);
    raw |= ((uint32_t)Data_3 << 16);
    raw |= ((uint32_t)Data_2 << 8);
    raw |= ((uint32_t)Data_1);

    // 将原始 4 字节解释为 float（IEEE 754）
    result = *((float *)&raw);

    return result;
}

void AHRSData2PC(void)
{
	Roll_Deg	= AHRSData_Packet.Roll		* 57.2958;
	Pitch_Deg	= AHRSData_Packet.Pitch		* 57.2958;
	Yaw_Deg		= AHRSData_Packet.Heading	* 57.2958;
	
	yaw_mapped = Yaw_Deg;
	if (yaw_mapped >= 180.0)
		yaw_mapped -= 360.0;

	
//	printf("%.2f, %.2f, %.2f\n",Roll_Deg, Pitch_Deg, Yaw_Deg);
}



